#ifndef SHOOTER_HPP
#define SHOOTER_HPP

#define MAX_ANGLE 180.0
#define MIN_ANGLE -45.0
#define STARTING_ANGLE 0
#define CAMERA_ITERATOR 5 // Slow down control of camera
#define SPEED_TABLE_LENGTH 10

#define PGAIN 100
#define IGAIN .003
#define DGAIN .5
#define ENCODER_LPR 250 //1X mode

class Shooter
{
	public:
	// Constructor
	Shooter(UINT32 pwmCh, UINT32 motID);
	
	~Shooter();	
	
	// Control the shooter motor by camera tilt angle
	// cameraY - value from -1 - +1 from the joystick camera tilt angle controller
	// cameraY is added, to increase or decrease, the current angle of the camera
	// and the shooter motor speed will be set based on the resulting angle. 
	void ControlByCamera(float cameraY);
	
	// Set the shooter motor speed manually, speed is a percent of max speed
	// e.g. 75 would be 75% of max speed
	// Can be used to turn off shooter motor (set speed to 0)
	void ManualControl(float speed);
	
	// Set the shooter motor speed manually, speed is a percent of max speed
	// e.g. 75 would be 75% of max speed
	// Can be used to turn off shooter motor (set speed to 0)
	void ManualControlIncremental(float incSpeed);
	
	// Sets the camera tilt angle. 0deg is looking forwar, 90deg is looking up
	// This does not actually move the camera it only sets the internal angle setting
	// To use this, move the camera manually to an angle and then command that angle here
	void CalibrateCameraAngle(float angle);
	
	// Returns the camera tilt angle. 0deg is looking forwar, 90deg is looking up
	float GetCameraAngle();
	
	private:
	void SetCameraAngle(float angle);
	float SpeedLookup(float angle);
	DriverStationLCD *lcd;
	
	Servo 		cameraServo;
	CANJaguar 	shooterMotor;
	
	float 	cameraAngle;
	float	cameraAngleCal;
	int 	cameraIterator;
	float 	speedTable[SPEED_TABLE_LENGTH][2];
	float 	motorSetpoint;
	bool 	manualControl;
};

#endif
